#include "Rough_tracking_calculation.h"
#include <AccelStepper.h>
#include <math.h>
#include <MultiStepper.h>
#define STEPPER1_DIR_PIN  6
#define STEPPER1_STEP_PIN 7
#define STEPPER2_DIR_PIN  8
#define STEPPER2_STEP_PIN 9
// 每个月的天数，注意闰年2月有29天
const int days_in_month[] = {0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};

long positions[2]; // 2个电机坐标位置

/********************************************************************************************************************************************
 *@*名称 ： Is_Leap_Year
 *@*功能 ： 判断是否为闰年
 *@*形参 ： YearData年数据
 *@*返回值 ： 0 or 1
 *********************************************************************************************************************************************/
int Is_Leap_Year(int YearData) {
    if ((YearData % 4 == 0 && YearData % 100 != 0) || YearData % 400 == 0) {
        return 1;
    }
    return 0;
}
/********************************************************************************************************************************************
 *@*名称 ： Days_Calculation
 *@*功能 ： 计算日期是当年的第几天
 *@*形参 ： YearData年数据   MonthData月数据  DayData日数据
 *@*返回值 ： Days天数
 *********************************************************************************************************************************************/
int Days_Calculation(int YearData, int MonthData, int DayData) {
    int i;
    int Days = DayData;

    for (i = 1; i < MonthData; i++) {
        Days += days_in_month[i];
    }
    // 如果是2月并且是闰年，需要加上一天
    if (MonthData > 2 && Is_Leap_Year(YearData)) {
        Days++;
    }

    return Days;
}
/********************************************************************************************************************************************
 *@*名称 ： Equation_Time_Calculation
 *@*功能 ： 计算均时差
 *@*形参 ： YearData年数据   MonthData月数据  DayData日数据
 *@*返回值 ： EquationTime均时差
 *********************************************************************************************************************************************/
double Equation_Time_Calculation(int YearData, int MonthData, int DayData) {
    double EquationTime; // 均时差，以小时为单位
    double B;
    int Days;

    Days = Days_Calculation(YearData, MonthData, DayData);
    B = (2 * PI * (Days - 81)) / 365.24;
    EquationTime = 9.87 * sin(2 * B) - 7.53 * cos(B) - 1.5 * sin(B); // 均时差公式 结果以弧度为单位
    EquationTime = (EquationTime * (180.0 / PI)) / 15.0 / 4.0;       // 换算弧度为度，再换算为分钟，除以4是因为sin和cos函数的值域是[-1, 1]
    EquationTime = EquationTime / 60.0;                              // 分钟换算小时

    return EquationTime; // 返回均时差 结果以小时为单位
}
/********************************************************************************************************************************************
 *@*名称 ： Declination_Angle_Calculation
 *@*功能 ： 计算太阳赤纬角
 *@*形参 ： YearData年数据   MonthData月数据  DayData日数据
 *@*返回值 ： DeclinationAngle太阳赤纬角
 *********************************************************************************************************************************************/
double Declination_Angle_Calculation(int YearData, int MonthData, int DayData) {
    int Days;
    double DeclinationAngle;

    Days = Days_Calculation(YearData, MonthData, DayData);
    DeclinationAngle = 23.45 * sin((2 * PI / 365.0) * (Days + 284.0)); // 赤纬角公式

    return DeclinationAngle; // 返回赤纬角
}
/********************************************************************************************************************************************
 *@*名称 ： Hour_Angle_Calculation
 *@*功能 ： 计算太阳时角
 *@*形参 ： YearData年数据   MonthData月数据   DayData日数据   HourData小时数据    LongitudeData经度数据
 *@*返回值 ： HourAngle太阳时角
 *********************************************************************************************************************************************/
double Hour_Angle_Calculation(int YearData, int MonthData, int DayData, double HourData, double LongitudeData) {
    double HourAngle;       // 时角
    double EquationTime;    // 均时差
    double True_Solar_Time; // 真太阳时

    EquationTime = Equation_Time_Calculation(YearData, MonthData, DayData);
    True_Solar_Time = HourData + LongitudeData / 15.0 + EquationTime;
    HourAngle = 15.0 * (True_Solar_Time - 12);

    return HourAngle;
}
/********************************************************************************************************************************************
 *@*名称 ： Height_Angle_Calculation
 *@*功能 ： 计算太阳高度角
 *@*形参 ： LongitudeData经度数据   LatitudeData纬度数据   YearData年数据   MonthData月数据   DayData日数据   HourData小时数据
 *@*返回值 ： ElevatingAngle太阳高度角
 *********************************************************************************************************************************************/
double Height_Angle_Calculation(double LongitudeData, double LatitudeData, int YearData, int MonthData, int DayData, double HourData) {
    double elevatingAngle;     // 高度角
    double ElevatingAngle_sin; // 高度角正弦值

    double DeclinationAngle;                                                        // 赤纬角
    DeclinationAngle = Declination_Angle_Calculation(YearData, MonthData, DayData); // 计算赤纬角

    double HourAngle;                                                                          // 时角
    HourAngle = Hour_Angle_Calculation(YearData, MonthData, DayData, HourData, LongitudeData); // 计算时角

    double DeclinationAngle_sin = sin(DeclinationAngle * PI / 180); // 赤纬角的正弦值
    double DeclinationAngle_cos = cos(DeclinationAngle * PI / 180); // 赤纬角余弦值
    double LatitudeData_sin = sin(LatitudeData * PI / 180);         // 纬度正弦值
    double LatitudeData_cos = cos(LatitudeData * PI / 180);         // 纬度余弦值
    double HourAngle_cos = cos(HourAngle * PI / 180);               // 时角余弦值

    ElevatingAngle_sin = (DeclinationAngle_sin * LatitudeData_sin) + (DeclinationAngle_cos * LatitudeData_cos * HourAngle_cos);
    elevatingAngle = asin(ElevatingAngle_sin) * 180 / PI;
    return elevatingAngle;
}
/********************************************************************************************************************************************
 *@*名称 ： Azimuth_Angle_Calculation
 *@*功能 ： 计算太阳方位角
 *@*形参 ： LongitudeData经度数据   LatitudeData纬度数据   YearData年数据   MonthData月数据   DayData日数据   HourData小时数据
 *@*返回值 ： 太阳方位角（0点开始360度）
 *********************************************************************************************************************************************/
double Azimuth_Angle_Calculation(double LongitudeData, double LatitudeData, int YearData, int MonthData, int DayData, double HourData) {
    double azimuthAngle;     // 方位角
    double AzimuthAngle_cos; // 方位角余弦值

    double elevatingAngle;                                                                                          // 高度角
    elevatingAngle = Height_Angle_Calculation(LongitudeData, LatitudeData, YearData, MonthData, DayData, HourData); // 计算高度角

    double HourAngle;                                                                          // 时角
    HourAngle = Hour_Angle_Calculation(YearData, MonthData, DayData, HourData, LongitudeData); // 计算时角

    double DeclinationAngle;                                                        // 赤纬角
    DeclinationAngle = Declination_Angle_Calculation(YearData, MonthData, DayData); // 计算赤纬角

    double ElevatingAngle_cos = cos(elevatingAngle * PI / 180);     // 高度角余弦值
    double ElevatingAngle_sin = sin(elevatingAngle * PI / 180);     // 高度角正弦值
    double LatitudeData_sin = sin(LatitudeData * PI / 180);         // 纬度正弦值
    double LatitudeData_cos = cos(LatitudeData * PI / 180);         // 纬度余弦值
    double HourAngle_sin = sin(HourAngle * PI / 180);               // 时角正弦值
    double DeclinationAngle_cos = cos(DeclinationAngle * PI / 180); // 赤纬角余弦值
    double DeclinationAngle_sin = sin(DeclinationAngle * PI / 180); // 赤纬角正弦值

    AzimuthAngle_cos = (ElevatingAngle_sin * LatitudeData_sin - DeclinationAngle_sin) / (ElevatingAngle_cos * LatitudeData_cos); // 方位角cos值
    azimuthAngle = acos(AzimuthAngle_cos) * 180 / PI;                                                                            // 方位角角度
	// 不判断的话12点为180度最高，然后两边的角度都是180度递减，所以需要一个360度的角度反馈需要将方位角判断一下
	// 因为小时是UTC时间，比北京时间晚8个小时，所以下午12-24点应该是对应UTC时间的4-16点
	if (HourData > 4 && HourData < 16) {
        return azimuthAngle + 180;
    } else {
        return -azimuthAngle + 180;
    }
}

// 测试内容
//	char buffer[50] = {0};
//	double LongitudeData = 118.084442333;
//	double LatitudeData = 24.625310667;
//	int YearData = 24;
//	int MonthData = 6;
//	int DayData = 22;
//	double HourData = 6.5;
//	double elevatingAngle;
//	double azimuthAngle;
//	
//	elevatingAngle = Height_Angle_Calculation( LongitudeData,  LatitudeData,  YearData,  MonthData,  DayData,  HourData);
//	azimuthAngle = Azimuth_Angle_Calculation( LongitudeData,  LatitudeData,  YearData,  MonthData,  DayData,  HourData);
//	

//    sprintf(buffer, "elevatingAngle: %.3f", elevatingAngle);
//    LCDShowString(10, 500, 500, 24, 24, buffer, BLUE);
//	sprintf(buffer, "azimuthAngle: %.3f", azimuthAngle);
//    LCDShowString(10, 525, 500, 24, 24, buffer, BLUE);





void RotationAngle(AccelStepper &stepper, float pulse) {

    stepper.moveTo(pulse);
}
void Rotation_BLE(AccelStepper &stepper, float pulse) 
{
    stepper.move(pulse);
}

void stepper_init(AccelStepper &stepperone,int MaxSpeed,int Acceleration) {
    stepperone.setCurrentPosition(0);          // 复位1#电机初始位置
    stepperone.setMaxSpeed(MaxSpeed);             // 设置1#电机最大速度
    stepperone.setAcceleration(Acceleration);         // 设置1#电机加速度
}
void stepperstop(AccelStepper &stepper) {
    stepper.stop();
}
void run_stepper(AccelStepper &stepper)
{
    stepper.runSpeedToPosition();
}